/* 
 * File:   Win3D.h
 * Author: hadrien
 *
 * Created on 6 mars 2012, 21:03
 */

#ifndef WIN3D_H
#define	WIN3D_H
#include <mrpt/base.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
#include <mrpt/opengl.h>
#include "global.h"

class Win3D {
public:
    Win3D();
    virtual ~Win3D();
    void repaint(
        const mrpt::slam::CRangeBearingKFSLAM2D                 &mapping, 
        const std::vector<mrpt::poses::TPose3D>                 &meanPath, 
        const std::vector<mrpt::poses::TPoint2D>                &LMs, 
        const mrpt::poses::CPosePDFGaussian                     &robotPose, 
        const mrpt::poses::CPose3D                              &robotPoseMean3D, 
        const mrpt::math::CMatrixDouble                         &GT_PATH, 
        const mrpt::slam::CLandmarksMap                         &landmarkMap, 
        const double                                            &tim_kf_iter, 
        const size_t                                            &step
    );
    void wait();
private:
    mrpt::gui::CDisplayWindow3DPtr win3d;
    
    static void landmark_to_3d(
        const mrpt::slam::CRangeBearingKFSLAM2D::KFArray_FEAT   &lm, 
        mrpt::poses::TPoint3D                                   &p
    );
};

#endif	/* WIN3D_H */

